#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#include <WiFi.h>
#include <esp_now.h>
#include <MAC_address.h>

//该条语句用于使能DEBUG输出信息，屏蔽掉就不会输出debug调试信息
// #define DEBUG
// #define CALLBACK //是否需要串口打印发送成功的信息，注释掉就是不需要
#define LED_pin 2
// 绑定数据接收端
esp_now_peer_info_t peerInfo;
String a_command, A_command, b_command, B_command, c_command, C_command, d_command, D_command; //用于存储控制不同电机的指令
// 设置数据结构体
typedef struct struct_message
{
    String board_name = "My_FOC";
    double A_motor1_angle = 0;
    double A_motor2_angle = 0;
    double B_motor1_angle = 0;
    double B_motor2_angle = 0;
    double C_motor1_angle = 0;
    double C_motor2_angle = 0;
    double D_motor1_angle = 0;
    double D_motor2_angle = 0;
} struct_message;

struct_message myFOC;
struct_message myFOC_1;

// 接收设备的 MAC 地址 1 58:BF:25:32:D2:0C 2 58:BF:25:33:48:5C 3 58:BF:25:33:2E:28 4 58:BF:25:33:1A:BC
uint8_t broadcastAddress1[6]; // 这里只是创建了一个空数组，我用的是自己写的一个头文件<MAC_address.h>，这个文件里面存储的是所有的板子的地址
uint8_t broadcastAddress2[6];
uint8_t broadcastAddress3[6];
uint8_t broadcastAddress4[6];
uint8_t broadcastAddress5[6];
// 数据发送回调函数
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
#ifdef CALLBACK
    char macStr[18];
    Serial.print("Packet to: ");
    snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
             mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
    Serial.println(macStr);
    Serial.print("Send status: ");
    Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
    Serial.println();
#endif
}
// 数据接收回调函数 几乎不会被用到
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len)
{
    memcpy(&myFOC, incomingData, sizeof(myFOC));
    // double motor1_angle = myFOC.A_motor1_angle;
    // double motor2_angle = myFOC.A_motor2_angle;
    // Serial.printf("电机一：%lf,电机二：%lf\n", motor1_angle, motor2_angle);
#ifdef DEBUG
    Serial.print("board_name: ");
    Serial.println(myFOC.board_name);
    Serial.print("motor1_angle: ");
    Serial.println(myFOC.A_motor1_angle);
    Serial.print("motor2_angle:");
    Serial.println(myFOC.A_motor2_angle);
    Serial.println();
#endif
}
void espNowInit()
{
    //将字符数组中存储的MAC地址转化为16进制数组中存储的数据,这样通过修改编号就可以决定与那个板子进行通讯
    mac_str_to_bin(address[0], broadcastAddress1);
    mac_str_to_bin(address[1], broadcastAddress2);
    mac_str_to_bin(address[2], broadcastAddress3);
    mac_str_to_bin(address[3], broadcastAddress4);
    mac_str_to_bin(address[15], broadcastAddress5);
    // Init ESP-NOW
    WiFi.mode(WIFI_STA);
    Serial.println(WiFi.macAddress());
    if (esp_now_init() != ESP_OK)
    {
        Serial.println("Error initializing ESP-NOW");
        return;
    }
    esp_now_register_recv_cb(OnDataRecv);

    esp_now_register_send_cb(OnDataSent);

    // Register peer
    // esp_now_peer_info_t peerInfo;
    peerInfo.channel = 0;
    peerInfo.encrypt = false;

    // Add peer
    memcpy(peerInfo.peer_addr, broadcastAddress5, 6);//只与第五块板子进行无线通信，然后第五块板子和其他的板子通过串口进行连接
    // 检查设备是否配对成功
    if (esp_now_add_peer(&peerInfo) != ESP_OK)
    {
        Serial.println("Failed to add peer1");
        return;
    }
}
String serialReceiveUserCommand()
{
    // a string to hold incoming data
    static String received_chars;

    String command = "";
    while (Serial.available() > 0)
    {
        // get the new byte:
        char inChar = (char)Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n')
        {
            // execute the user command
            command = received_chars;
            int a_indent = command.indexOf('a'); //变量1
            int A_indent = command.indexOf('A'); //变量2
            int b_indent = command.indexOf('b'); //变量3
            int B_indent = command.indexOf('B'); //变量3
            int c_indent = command.indexOf('c'); //变量1
            int C_indent = command.indexOf('C'); //变量2
            int d_indent = command.indexOf('d'); //变量3
            int D_indent = command.indexOf('D'); //变量3
            a_command = command.substring(a_indent + 1, A_indent).c_str();
            A_command = command.substring(A_indent + 1, b_indent).c_str();
            b_command = command.substring(b_indent + 1, B_indent).c_str();
            B_command = command.substring(B_indent + 1, c_indent).c_str();
            c_command = command.substring(c_indent + 1, C_indent).c_str();
            C_command = command.substring(C_indent + 1, d_indent).c_str();
            d_command = command.substring(d_indent + 1, D_indent).c_str();
            D_command = command.substring(D_indent + 1, command.length()).c_str();
#ifdef DEBUG
            Serial.printf("FOC1 电机1:%s,电机2:%s;\nFOC2 电机1:%s,电机2:%s;\nFOC3 电机1:%s,电机2:%s;\nFOC4 电机1:%s,电机2:%s;\n", a_command.c_str(), A_command.c_str(), b_command.c_str(), B_command.c_str(), c_command.c_str(), C_command.c_str(), d_command.c_str(), D_command.c_str());
#endif
            received_chars = "";
        }
    }
    return command;
}
void sendData()
{
    int delayms = 3;
    // 发送数据
    ///////////////////////这种发送的方式，没有一个一个发的效果那么好///////////////////////
    // esp_now_send()中的第一个参数为0表示向所有接收的板子发送，也是传入指定的板子地址
    // esp_err_t result = esp_now_send(0, (uint8_t *)&myFOC_1, sizeof(myFOC_1));
    // // 检查数据是否发送成功
    // if (result == ESP_OK)
    // {
    //     // Serial.println("Sent 1 with success");
    // }
    // else
    // {
    //     Serial.println("Error sending the data 1");
    // }
    ////////////////////////////////////////////////////////////////////////////////////
    myFOC_1.board_name = "FOC1";
    myFOC_1.A_motor1_angle = a_command.toDouble();
    myFOC_1.A_motor2_angle = A_command.toDouble();
    myFOC_1.B_motor1_angle = b_command.toDouble();
    myFOC_1.B_motor2_angle = B_command.toDouble();
    myFOC_1.C_motor1_angle = c_command.toDouble();
    myFOC_1.C_motor2_angle = C_command.toDouble();
    myFOC_1.D_motor1_angle = d_command.toDouble();
    myFOC_1.D_motor2_angle = D_command.toDouble();
    esp_err_t result1 = esp_now_send(broadcastAddress5, (uint8_t *)&myFOC_1, sizeof(myFOC_1));
    // 检查数据是否发送成功
    if (result1 == ESP_OK)
    {
        // Serial.println("Sent 1 with success");
    }
    else
    {
#ifdef DEBUG
        Serial.println("Error sending the data 1");
#endif
    }
    delay(delayms);
}
void task_LED(void *pvParam) // 宣告任务LED闪烁
{
	/* 使用ledc通道1，设置频率为1kHz，分辨率为10位 */
	int freq = 1000;					  // 频率
	int channel = 0;					  // 通道
	int resolution = 8;					  // 分辨率
	ledcSetup(channel, freq, resolution); // 设置通道
	ledcAttachPin(LED_pin, channel);	  // 将通道与对应的引脚连接
	while (1)
	{
		// 逐渐变亮
		for (int dutyCycle = 0; dutyCycle <= 255; dutyCycle = dutyCycle + 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}

		// 逐渐变暗
		for (int dutyCycle = 255; dutyCycle >= 0; dutyCycle = dutyCycle - 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}
	}
}
void setup()
{
    xTaskCreate(task_LED, "task_LED", 1000, NULL, 1, NULL);
    Serial.begin(115200);
    // 初始化 ESP-NOW
    espNowInit();
}
void loop()
{
    // 设置要发送的数据
    serialReceiveUserCommand();
    sendData();
}
// 该例子演示的是将其中一块 ESP32 开发板（此处以掌控板为例）检测到的数据发送给另一块 ESP32 开发板。
// 首先在开头引入了 ESP-NOW 相关的头文件：WiFi.h 与 esp_now.h。然后定义了传感器的引脚和一个名为 myFOC_1 的结构体，该结构体由 3 个不同数据组成，分别是开发板名称（board_name），光线值（light）与声音值（motor2_angle），其中开发板名称为字符串类型，光线值与声音值为浮点型数据。若我们同时有多个传感器数据检测端，可以通开发板名称来区分来自设备的数据。
// 接着用uint8_t broadcastAddress1[] = {0x24, 0x6F, 0x28, 0x88, 0x62, 0x80} 定义了接收设备的 MAC 地址。
// 除此之外，我们注册了一个 OnDataSent() 的数据发送回调函数，该函数反馈了 ESP-NOW 数据的发送状态，该例子每隔一秒将光线值和声音值发送到指定 MAC 地址的开发板。
// 在 setup() 中，我们先是初始化了 ESP-NOW 相关的功能，然后设置了发送数据的回调函数、以及绑定了数据接收端的 MAC 地址。
// 在 loop() 中，我们不断读取光线值和声音值，并将他们赋值给 myFOC_1 结构体，然后将它们发送到接收端，并且去判断数据是否发送成功。
